#include "Math.h"


SVec3 CMath::ProjectPointToPlane(const SVec3 &InPoint, const SVec3 &InPlaneNormal, const SVec3 &InPlanePoint)
{
    auto PlaneNormal = InPlaneNormal;
    PlaneNormal.Normalize();

    SVec3 LineToPlane = InPlanePoint - InPoint;
    float Distance = LineToPlane.DotProduct(PlaneNormal);
    return InPoint + PlaneNormal * Distance;

}

bool CMath::LineLineIntersection(const SVec3 &InLine1Point1, const SVec3 &InLine1Direction, const SVec3 &InLine2Point2, const SVec3 &InLine2Direction, SVec3 &OutIntersectionPoint)
{
    auto Line1Direction = InLine1Direction;
    Line1Direction.Normalize();

    auto Line2Direction = InLine2Direction;
    Line2Direction.Normalize();

    //直接套公式： 3d数学基础 P268
    //L1: P=P1+t1*d1
    //L2: P=P2+t2*d2
    //t1=(P2-P1) X (d2)·(d1 X d2)/||d1 X d2||^2
    
    SVec3 D1XD2 = InLine1Direction.CrossProduct(InLine2Direction);

    float D1XD2LengthSquared = D1XD2.GetLengthSquared();
    if (D1XD2LengthSquared < 0.0001f)
    {
        //两直线平行
        return false;
    }

    SVec3 P2P1 = InLine2Point2 - InLine1Point1;
    float T1= P2P1.CrossProduct(InLine2Direction).DotProduct(D1XD2) / D1XD2LengthSquared;

    OutIntersectionPoint = InLine1Point1 + InLine1Direction * T1;
    return true;
}

SVec3 CMath::DeprojectViewportToScene(
    const SVec2 &InViewportPos, float InViewportWidth, float InViewportHeight, const SMatrix4x4 &InProjectionMatrix, const SMatrix4x4 &InViewMatrix, SVec3 &OutWorldDirection)
{
    //裁剪空间坐标ClipPos= Project*View*World;
    //设备坐标NDCPos=ClipPos/ClipPos.W;
    //那么世界坐标WorldPos=(Project*View).Inverse() * (NDCPos*ClipPos.W);
    //其中关键是求ClipPos.W
    //ClipPos.W= (Project*View*World).W;
    //由于视图矩阵View最后一行为(0,0,0,1),不影响W分量
    //，而且世界坐标World的W分量为1, 所以可以简化为
    //ClipPos.W
    //  = (Project*World).W
    //  =  Project[0][3]*World.X
        //+Project[1][3]*World.Y
        //+Project[2][3]*World.Z
        //+Project[3][3]*1
    
    //对于正交投影矩阵
    //由于Project[0][3]=0 Project[1][3]=0 Project[2][3]=0 Project[3][3]=1
    //所以ClipPos.W=1
    //NDCPos=ClipPos/ClipPos.W
    //      =Project*View*World
    //(Project*View).Inverse() *NDCPos=World

    //对于透视投影矩阵
    //由于 Project[0][3]=0 Project[2][3]=0 Project[3][3]=0
    //所以 ClipPos.W=Project[1][3]*World.Y
    //NDCPos=ClipPos/ClipPos.W
    //      =Project*View*World / (Project[1][3]*World.Y)
    //(Project*View).Inverse() *NDCPos=World / World.Y=(World.X/World.Y,1,World.Z/World.Y,1/World.Y)

    //将屏幕坐标转换为vulkan NDC坐标
    SVec4 VulkanNDCPos;
    VulkanNDCPos.X = (InViewportPos.X / InViewportWidth - 0.5f) * 2.0f;
    VulkanNDCPos.Y = (InViewportPos.Y / InViewportHeight - 0.5f) * 2.0f;
    VulkanNDCPos.Z = 0.0f; //近平面
    VulkanNDCPos.W = 1.0f;

    SMatrix4x4 InverseProjectionViewMatrix = (InProjectionMatrix * InViewMatrix).Inverse();

    SVec4 WorldPos = InverseProjectionViewMatrix * VulkanNDCPos;

    if(InProjectionMatrix.Get(1,3)!=0)
    {
        //这是透视投影矩阵
        WorldPos /=WorldPos.W;
    }

    SVec3 Result=ToVec3(WorldPos);
    
    //计算世界坐标系下鼠标射线方向
    //找个离近平面稍远一点的点，
    //不能选远平面上的点，因为远平面上的点数字太大，会导致精度丢失
    SVec4 LittleFarPos = InverseProjectionViewMatrix * SVec4(VulkanNDCPos.X, VulkanNDCPos.Y, 0.1f, 1.0f);
    if(InProjectionMatrix.Get(1,3)!=0)
    {
        LittleFarPos /=LittleFarPos.W;
    }
    OutWorldDirection = ToVec3(LittleFarPos) - Result;
    OutWorldDirection.Normalize();
    return Result;
}

SVec2 CMath::ProjectSceneToViewport(const SVec3 &InWorldPos, float InViewportWidth, float InViewportHeight, const SMatrix4x4 &InProjectionMatrix, const SMatrix4x4 &InViewMatrix)
{
    //将世界坐标转换为裁剪坐标
    SVec4 ClipPos = InProjectionMatrix * InViewMatrix * SVec4(InWorldPos, 1.0f);

    //将裁剪坐标转换为vulkan NDC坐标
    ClipPos /= ClipPos.W;

    //将vulkan NDC坐标转换为屏幕坐标
    SVec2 ScreenPos;
    ScreenPos.X = (ClipPos.X * 0.5f + 0.5f) * InViewportWidth;
    ScreenPos.Y = (ClipPos.Y * 0.5f + 0.5f) * InViewportHeight;

    return ScreenPos;

}
